/*
 *  definitions.h
 *  CommandCenter08
 *
 *  Created by Matt Roman & David Miller on 7/7/08.
 *  Copyright 2008 University of Oklahoma. All rights reserved.
 *
 */


#define	PI				3.14159265
#define TWOPI			2*PI
#define HALFPI 			1.57079633
#define SQRTTWO			1.41421356
#define DEGTORAD(deg)	(((deg) * PI) / 180.0)
#define RADTODEG(rad)	(((rad) * 180.0) / PI)
#define TRUE			1
#define FALSE			0
#define MINIMUM(x,y)	(((x) < (y)) ? (x) : (y))
#define MAXIMUM(x,y)	(((x) > (y)) ? (x) : (y))
#define SQ(x)			((x)*(x)) 
//codes for an alarm messages
#define UPDATETOOFAR 	1
#define POINTTURNSTALL	2
#define MAXNUMOBSTACLES 100
#define MAXL	1
#define MAXW	1
#define MAXH	0.5

typedef struct _konstants{
	float	DECLINATION; // ~ 12.45 degrees
	float	WHEELDIAM; //0.2
	int		LTICSPERREV;//4200
	int		RTICSPERREV;//4200
	//rover width in meters and decimeters
	float	ROVERWIDTH;// 0.5
	int		ROVERWIDTHINT;// 5
	// if rover's new position is furthere than this from previous position, ring an alarm
	float	TELEPORTDISTANCE;//1.0
	// close enough (in meters) to waypoint to count as having made it there
	float	CLOSEENOUGH ;//1.0
	//seconds time limit where rover needs to be closer to waypoint then it was before
	int		NOPROGRESSTIME;//600
	float	STALLPERCENTAGE;//0.25
	float	TURNDIFF ;//700
	float	POINTTURNANGLE 	;//30.0
	float	POINTTURNSPEED 	;//750
	int		PTSTALLTIME;	//3000 stall timer in milliseconds
	float	TURNMULT;//		6
	//defining when turns are made
	float	NEEDTURNCORRECTION;//	8.0
	float	TURNACCURACYLIMIT;//	4.0
	int		ROVERCRUISESPEED;//	1600
	float	TURNINCREMENT;//	0.05
	int		JOYINCREMENT;//50
	int		SPEEDLIMIT;//	1601
	// distance considered far enough for bodyLaser (past this potential obstacles are ignored
	int		BODYDIST;// 1500
	//How high is the profile laser off flat ground? Same for panel laser
	float	PROFILEHEIGHT;// 0.46
	float	PANELHEIGHT;// 0.47
	//How high or low from flat ground does something have to be to be considered an obstacle?
	float	PROFILEOBSTACLEMAX;// 0.15
	float	PROFILEOBSTACLEMIN;// -0.15
	//How many of the 16 profile laser segments do you look at?
	int		PROFILESEG;// 12
	//How many PANELSEGMENTS are there? = 16
	int		PANELSEGMENTS;// 16
	// Angle between segments in profile laser = PI/32
	float	PROFILEINCR;// 0.09817477
	//Angle between segments of  Body and panel lasers between sgements = 22pi/512 = 0.13499
	float	BODYINCR;// 0.13499
	float	PANELINCR;// 0.13499
	float	MAXPITCH;// 15.0
	float	MAXROLL;// 20.0
	//Body laser should panic if reading is shorter than this
	float	BODYTOOCLOSE;// 0.5	
	//Forward offsets from rover differential of
	float	FOROFFSETWHEELS;//		0.35
	float	FOROFFSETBODYLASER;//	0.265
	float	FOROFFSETPROFILE;//	0.415
	float	FOROFFSETPANELLASER;//	0.500
	//Where does the panel laser intersect flat ground?
	float	FORPANELGROUNDINTERSECT;// (FOROFFSETPANELLASER+PANELHEIGHT)
	// distance in decimeters from rover where obstacles are considered active
	int		MAXDISTMAP;// 30
	int		NAPTIME; //10 minutes
	float	PATHEFFICIENCY; // used to check progress
	float	TURNFACTOR; // slow wheel % when turning
	float	GOPASTDIST;//how far past obstacle before goto WP
	int		PANELOBSTACLES;
	int		BODYOBSTACLES;
	int		PROFILEOBSTACLES;
	int		SLOPEOBSTACLES;
	float	PITCHDOWNIGNOREDISTOBSTACLES;//if pitch is less than this, ignore distant obstacles in front of rover (probably bottom of hill)
}konstants;

typedef struct _coordAngle {
	int deg;
	double min;
	double dd;
} coordAngle;

typedef	struct _LatLonCoord {
	coordAngle lat;
	coordAngle lon;
} LatLonCoord;

typedef	struct _Point3D {
	double	x;
	double	y;
	double	z;
} Point3D;

typedef struct _Vector3D {
	double x,y,z;
} Vector3D;

typedef struct _Ray3D{
	Point3D P0;
	Point3D P1;
} Ray3D;

typedef struct _Triangle3D{
	Point3D V0;
	Point3D V1;
	Point3D V2;
} Triangle3D;

typedef struct _frameVectors{
	Vector3D X,Y,Z;
} frameVectors;

typedef	enum _RoverState{
	RSStopped = 0, //Rover is stopped (may or may not be remaining WP). State for resetting position, etc
	RSMovingTowardsWaypoint = 1, // Normal state when moving
	RSNoRemainingWaypoints = 2, // No plan or plan is finished
	RSGoingToSleep = 3, //Power monitor or user has told rover to sleep
	RSWakingFromSleep = 4, // after mac has woke up but while senors, etc are booting
	RSDoingScience = 5, // taking pictures, etc.  Rover is not moving
	RSCallingForHelp = 6, // progress alarm, long stalls, or teleport problems
	RSInTeleopMode = 7, // when being teleoperated
	RSAvoidingNearbyObstacles = 8, // when path is being modified to avoid obstacles sensed by panel or profile laser
	RSAvoidingDistantObstacles = 9, // when path is being modified to avoid obstacles sensed by body laser
	RSReachedWaypoint = 10, // a momentary state when the point is reached but science or next point not started
	RSGoingPastObstacles = 11  // the rover is ignoring the current waypoint direction in order to drive clear of an obstacle
} RoverState;

typedef	enum _RoverError{
	RENone = 0,
	REPosition = 1,
	REStall = 2,
	REProgress = 3,
	REPitch = 4,
	RERoll = 5
} RoverError;

typedef	enum _WPState{
	WPStateNew = 0, // to be visited
	WPStateOld = 1, // visited
	WPStateCurrent = 2,
	WPStateSkipped = 3
} WPState;

typedef enum _WPScienceType {
    WPScienceNone = 0,
    WPSciencePanorama = 1,
    WPScienceSpectra = 2,
    WPSciencePanoramaAndSpectra = 3
} WPScienceType;

typedef struct _wayPoint{
	int			uuid; //just a number so we can tell one from another
	Point3D		position;
	WPScienceType	science;
	WPState		state; // to be visited, visitied, current, etc
	struct _wayPoint *next; //linked list so this is the next point the rover should visit
}wayPoint;

typedef enum _SenseType{
	SenseCompass = 0,
	SenseBodyLaser = 1,
	SensePanelLaser = 2,
	SenseProfileLaser = 3,
	SensePanelHeight = 4,
	SenseProfileHeight = 5
} SenseType;

typedef struct _laserScanner {	
	int		startPoint;
	int		stopPoint;
	int		grouping;
	int		rayCount;
	float	alpha;		// start point reference from X axis is 0 angle, in radians
	float	dAlpha;		// the angle between scan points in radians
	Point3D	origin;		// origin of laser reference frame
	frameVectors frame;	// reference frame vectors
	int		*rangeData;
}laserScanner;


typedef struct _obstacleCell{
	Point3D		position; //coordinates and height deviation of obstacle
	int			xIdx; // X coordinate in decimeters trunacted
	int			yIdx; //Y coordinate in decimeters
	int		active; //TRUE if cell is being used and false if it can be replaced (too far from rover)
}obstacleCell;


typedef struct _roverProperties {
	RoverState	state;
	RoverError	error;
	float		voltage;
	float		electronicsCurrent;
	float		panelCurrent;
	float		motorCurrent;
	float		compass;		// angle in degrees
	float		tilt;
	float		roll;
	LatLonCoord GPS;
	Point3D		position;
	float		leftSpeed;
	float		rightSpeed;
	int			leftTicks;
	int			rightTicks;
	float		odometer;
	int			leftStall; // set in updateRoverPosition
	int			rightStall;// set in updateRoverPosition
	int			turnStall;
	int			xbctime; //filled by XBC mseconds()
	int			turnAvoidDirection;//if avoiding an obstacle, this is the turn direction -1 left, 1 right 0 none.
	laserScanner *bodyScanner;
	laserScanner *panelScanner;
	laserScanner *profileScanner;
	obstacleCell *nearMap; // an array of size MAXNUMOBSTACLES of type obstacleCell
	wayPoint	*wp; 
}roverProperties;

//*********GLOBAL*******
konstants rk;

//***********FUNCTIONS*********
Point3D MakePoint(double a,double b,double c);
Vector3D MakeVector(double a,double b,double c);
double magnitude3d(Point3D p,Point3D q);
double dot(Vector3D V1,Vector3D V2);
double dotVP(Vector3D V,Point3D P);
void generateRotationFrame(frameVectors *FV);
float cross(Vector3D V1,Vector3D V2, Vector3D*N);
float NormalizeCross(Vector3D *v1, Vector3D *v2, Vector3D *v3);
void pointsToVector(Point3D a,Point3D b,Vector3D*V);
void arraysToVector(float *a,float *b,Vector3D*V);
void defaultKonstants(void);
